#include <stdint.h>
#include "stm32f4xx_hal.h"


#ifndef PWMServoDriver_H
#define PWMServoDriver_H



#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4

#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE

#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9

#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD


class PWMServoDriver {
 public:
  PWMServoDriver(I2C_HandleTypeDef *_p_i2c, uint8_t addr = 0x80);

  void reset(void);
  void setPWMFreq(float freq);
  void setPWM(uint8_t num, float angle, float servo_max_angle);

 private:
  I2C_HandleTypeDef *p_i2c;
  uint8_t i2caddr_write;

  uint8_t read8(uint8_t addr);
  void write8(uint8_t addr, uint8_t d);
};

#endif
